﻿/*
* Copyright (c) 2010-2012 Tesla Engine Group
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
* THE SOFTWARE.
*/


using System;
using System.Globalization;
using System.Runtime.InteropServices;
using Tesla.Util;

namespace Tesla.Math {

    /// <summary>
    /// Defines a 4 component vector.
    /// </summary>
    [Serializable]
    [StructLayout(LayoutKind.Sequential)]
    public struct Vector4 : IEquatable<Vector4> {

        /// <summary>
        /// X component of this vector.
        /// </summary>
        public float X;

        /// <summary>
        /// Y component of this vector.
        /// </summary>
        public float Y;

        /// <summary>
        /// Z component of this vector.
        /// </summary>
        public float Z;

        /// <summary>
        /// W component of this vector.
        /// </summary>
        public float W;

        private static Vector4 _zero = new Vector4(0f, 0f, 0f, 0f);
        private static Vector4 _one = new Vector4(1f, 1f, 1f, 1f);
        private static Vector4 _unitX = new Vector4(1f, 0f, 0f, 0f);
        private static Vector4 _unitY = new Vector4(0f, 1f, 0f, 0f);
        private static Vector4 _unitZ = new Vector4(0f, 0f, 1f, 0f);
        private static Vector4 _unitW = new Vector4(0f, 0f, 0f, 1f);

        private static int _sizeInbytes = MemoryHelper.SizeOf<Vector4>();

        /// <summary>
        /// Gets a Vector4 set to (0, 0, 0, 0).
        /// </summary>
        public static Vector4 Zero {
            get {
                return _zero;
            }
        }

        /// <summary>
        /// Gets a Vector4 set to (1, 1, 1, 1).
        /// </summary>
        public static Vector4 One {
            get {
                return _one;
            }
        }

        /// <summary>
        /// Gets a unit Vector4 set to (1, 0, 0, 0).
        /// </summary>
        public static Vector4 UnitX {
            get {
                return _unitX;
            }
        }

        /// <summary>
        /// Gets a unit Vector4 set to (0, 1, 0, 0).
        /// </summary>
        public static Vector4 UnitY {
            get {
                return _unitY;
            }
        }

        /// <summary>
        /// Gets a unit Vector4 set to (0, 0, 1, 0).
        /// </summary>
        public static Vector4 UnitZ {
            get {
                return _unitZ;
            }
        }

        /// <summary>
        /// Gets a unit Vector4 set to (0, 0, 0, 1).
        /// </summary>
        public static Vector4 UnitW {
            get {
                return _unitW;
            }
        }

        /// <summary>
        /// Gets the size of the Vector4 structure in bytes.
        /// </summary>
        public static int SizeInBytes {
            get {
                return _sizeInbytes;
            }
        }

        /// <summary>
        /// Creates a new instance of Vector4.
        /// </summary>
        /// <param name="value">Value to initialize each component to</param>
        public Vector4(float value) {
            X = Y = Z = W = value;
        }

        /// <summary>
        /// Creates a new instance of Vector4.
        /// </summary>
        /// <param name="value">Vector2 that contains x,y components</param>
        /// <param name="z">Z component</param>
        /// <param name="w">W component</param>
        public Vector4(Vector2 value, float z, float w) {
            X = value.X;
            Y = value.Y;
            Z = z;
            W = w;
        }

        /// <summary>
        /// Creates a new instance of Vector4.
        /// </summary>
        /// <param name="value">Vector4 that contains x,y,z components</param>
        /// <param name="w">W component</param>
        public Vector4(Vector3 value, float w) {
            X = value.X;
            Y = value.Y;
            Z = value.Z;
            W = w;
        }

        /// <summary>
        /// Creates a new instance of Vector4.
        /// </summary>
        /// <param name="x">X component</param>
        /// <param name="y">Y component</param>
        /// <param name="z">Z component</param>
        /// <param name="w">W component</param>
        public Vector4(float x, float y, float z, float w) {
            X = x;
            Y = y;
            Z = z;
            W = w;
        }

        
        /// <summary>
        /// Adds two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Add(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X + b.X;
            v.Y = a.Y + b.Y;
            v.Z = a.Z + b.Z;
            v.W = a.W + b.W;
            return v;
        }

        /// <summary>
        /// Adds two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Add(ref Vector4 a, ref Vector4 b, out Vector4 result) {
            result.X = a.X + b.X;
            result.Y = a.Y + b.Y;
            result.Z = a.Z + b.Z;
            result.W = a.W + b.W;
        }

        /// <summary>
        /// Subtracts a vector from a vector.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Subtract(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X - b.X;
            v.Y = a.Y - b.Y;
            v.Z = a.Z - b.Z;
            v.W = a.W - b.W;
            return v;
        }

        /// <summary>
        /// Subtracts a vector from a vector.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Subtract(ref Vector4 a, ref Vector4 b, out Vector4 result) {
            result.X = a.X - b.X;
            result.Y = a.Y - b.Y;
            result.Z = a.Z - b.Z;
            result.W = a.W - b.W;
        }

        /// <summary>
        /// Multiplies components of two vectors together.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Multiply(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X * b.X;
            v.Y = a.Y * b.Y;
            v.Z = a.Z * b.Z;
            v.W = a.W * b.W;
            return v;
        }

        /// <summary>
        /// Multiplies components of two vectors together.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Multiply(ref Vector4 a, ref Vector4 b, out Vector4 result) {
            result.X = a.X * b.X;
            result.Y = a.Y * b.Y;
            result.Z = a.Z * b.Z;
            result.W = a.W * b.W;
        }

        /// <summary>
        /// Multiplies components of a vector by a scalar value.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="scale">Scalar</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Multiply(Vector4 value, float scale) {
            Vector4 v;
            v.X = value.X * scale;
            v.Y = value.Y * scale;
            v.Z = value.Z * scale;
            v.W = value.W * scale;
            return v;
        }

        /// <summary>
        /// Multiplies components of a vector by a scalar value.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="scale">Scalar</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Multiply(ref Vector4 value, float scale, out Vector4 result) {
            result.X = value.X * scale;
            result.Y = value.Y * scale;
            result.Z = value.Z * scale;
            result.W = value.W * scale;
        }

        /// <summary>
        /// Divides the components of one vector by the components of the other.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Divisor vector</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Divide(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X / b.X;
            v.Y = a.Y / b.Y;
            v.Z = a.Z / b.Z;
            v.W = a.W / b.W;
            return v;
        }

        /// <summary>
        /// Divides the components of one vector by the components of the other.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Divisor vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Divide(ref Vector4 a, ref Vector4 b, out Vector4 result) {
            result.X = a.X / b.X;
            result.Y = a.Y / b.Y;
            result.Z = a.Z / b.Z;
            result.W = a.W / b.W;
        }

        /// <summary>
        /// Divides the components of one vector by a scalar.
        /// </summary>
        /// <param name="value">First vector</param>
        /// <param name="divisor">Divisor scalar</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Divide(Vector4 value, float divisor) {
            float invDiv = 1.0f / divisor;
            Vector4 v;
            v.X = value.X * invDiv;
            v.Y = value.Y * invDiv;
            v.Z = value.Z * invDiv;
            v.W = value.W * invDiv;
            return v;
        }

        /// <summary>
        /// Divides the components of one vector by a scalar.
        /// </summary>
        /// <param name="value">First vector</param>
        /// <param name="divisor">Divisor scalar</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Divide(ref Vector4 value, float divisor, out Vector4 result) {
            float invDiv = 1.0f / divisor;
            result.X = value.X * invDiv;
            result.Y = value.Y * invDiv;
            result.Z = value.Z * invDiv;
            result.W = value.W * invDiv;
        }

        /// <summary>
        /// Flip the signs of the components of this vector.
        /// </summary>
        public void Negate() {
            this.X = -this.X;
            this.Y = -this.Y;
            this.Z = -this.Z;
            this.W = -this.W;
        }

        /// <summary>
        /// Flip the signs of the vector's components.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Negate(Vector4 value) {
            Vector4 v;
            v.X = -value.X;
            v.Y = -value.Y;
            v.Z = -value.Z;
            v.W = -value.W;
            return v;
        }

        /// <summary>
        /// Flip the signs of the vector's components.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Negate(ref Vector4 value, out Vector4 result) {
            result.X = -value.X;
            result.Y = -value.Y;
            result.Z = -value.Z;
            result.W = -value.W;
        }

        /// <summary>
        /// Compute the length (magnitude) of this vector.
        /// </summary>
        /// <returns>Length</returns>
        public float Length() {
            float lengthSquared = (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W);
            return (float) System.Math.Sqrt((double) lengthSquared);
        }

        /// <summary>
        /// Compute length (magnitude), squared, of this vector.
        /// </summary>
        /// <returns></returns>
        public float LengthSquared() {
            return (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W);
        }

        /// <summary>
        /// Compute the dot product between two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Dot product</returns>
        public static float Dot(Vector4 a, Vector4 b) {
            return (a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z) + (a.W * b.W);
        }

        /// <summary>
        /// Compute the dot product between two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Dot(ref Vector4 a, ref Vector4 b, out float result) {
            result = (a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z) + (a.W * b.W);
        }

        /// <summary>
        /// Normalize this vector to a unit vector, which results in a vector with a count of 1 but
        /// perserves the direction the vector was pointing in.
        /// </summary>
        public void Normalize() {
            float invLength = 1.0f / (float) System.Math.Sqrt((this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W));
            this.X *= invLength;
            this.Y *= invLength;
            this.Z *= invLength;
            this.W *= invLength;
        }

        /// <summary>
        /// Normalize the surce vector to a unit vector, which results in a vector with a count of 1 but
        /// perserves the direction the vector was pointing in.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <returns>Resulting unit vector</returns>
        public static Vector4 Normalize(Vector4 value) {
            float invLength = 1.0f / (float) System.Math.Sqrt((value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z) + (value.W * value.W));
            Vector4 v;
            v.X = value.X * invLength;
            v.Y = value.Y * invLength;
            v.Z = value.Z * invLength;
            v.W = value.W * invLength;
            return v;
        }

        /// <summary>
        /// Normalize the surce vector to a unit vector, which results in a vector with a count of 1 but
        /// perserves the direction the vector was pointing in.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Normalize(ref Vector4 value, out Vector4 result) {
            float invLength = 1.0f / (float) System.Math.Sqrt((value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z) + (value.W * value.W));
            result.X = value.X * invLength;
            result.Y = value.Y * invLength;
            result.Z = value.Z * invLength;
            result.W = value.W * invLength;
        }

        /// <summary>
        /// Compute the distance between two vectors.
        /// </summary>
        /// <param name="start">First vector</param>
        /// <param name="end">Second vector</param>
        /// <returns>Resulting distance</returns>
        public static float Distance(Vector4 start, Vector4 end) {
            float dx = start.X - end.X;
            float dy = start.Y - end.Y;
            float dz = start.Z - end.Z;
            float dw = start.W - end.W;
            float distanceSquared = (dx * dx) + (dy * dy) + (dz * dz) + (dw * dw);
            return (float) System.Math.Sqrt((double) distanceSquared);
        }

        /// <summary>
        /// Compute the distance between two vectors.
        /// </summary>
        /// <param name="start">First vector</param>
        /// <param name="end">Second vector</param>
        /// <param name="result">Resulting distance</param>
        public static void Distance(ref Vector4 start, ref Vector4 end, out float result) {
            float dx = start.X - end.X;
            float dy = start.Y - end.Y;
            float dz = start.Z - end.Z;
            float dw = start.W - end.W;
            float distanceSquared = (dx * dx) + (dy * dy) + (dz * dz) + (dw * dw);
            result = (float) System.Math.Sqrt((double) distanceSquared);
        }

        /// <summary>
        /// Compute the distance squared between two vectors.
        /// </summary>
        /// <param name="start">First vector</param>
        /// <param name="end">Second vector</param>
        /// <returns>Resulting distance squared</returns>
        public static float DistanceSquared(Vector4 start, Vector4 end) {
            float dx = start.X - end.X;
            float dy = start.Y - end.Y;
            float dz = start.Z - end.Z;
            float dw = start.W - end.W;
            return (dx * dx) + (dy * dy) + (dz * dz) + (dw * dw);
        }

        /// <summary>
        /// Compute the distance squared between two vectors.
        /// </summary>
        /// <param name="start">First vector</param>
        /// <param name="end">Second vector</param>
        /// <param name="result">Resulting distance squared</param>
        public static void DistanceSquared(ref Vector4 start, ref Vector4 end, out float result) {
            float dx = start.X - end.X;
            float dy = start.Y - end.Y;
            float dz = start.Z - end.Z;
            float dw = start.W - end.W;
            result = (dx * dx) + (dy * dy) + (dz * dz) + (dw * dw);
        }

        /// <summary>
        /// Get the vector that contains the maximum value from each of the components of the 
        /// two supplied vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Maximum vector</returns>
        public static Vector4 Max(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = (a.X > b.X) ? a.X : b.X;
            v.Y = (a.Y > b.Y) ? a.Y : b.Y;
            v.Z = (a.Z > b.Z) ? a.Z : b.Z;
            v.W = (a.W > b.W) ? a.W : b.W;
            return v;
        }

        /// <summary>
        /// Get the vector that contains the maximum value from each of the components of the 
        /// two supplied vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Max(ref Vector4 a, ref Vector4 b, out Vector4 result) {
            result.X = (a.X > b.X) ? a.X : b.X;
            result.Y = (a.Y > b.Y) ? a.Y : b.Y;
            result.Z = (a.Z > b.Z) ? a.Z : b.Z;
            result.W = (a.W > b.W) ? a.W : b.W;
        }

        /// <summary>
        /// Get the vector that contains the mininum value from each of the components of the 
        /// two supplied vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Minimum vector</returns>
        public static Vector4 Min(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = (a.X < b.X) ? a.X : b.X;
            v.Y = (a.Y < b.Y) ? a.Y : b.Y;
            v.Z = (a.Z < b.Z) ? a.Z : b.Z;
            v.W = (a.W < b.W) ? a.W : b.W;
            return v;
        }

        /// <summary>
        /// Get the vector that contains the mininum value from each of the components of the 
        /// two supplied vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Min(ref Vector4 a, ref Vector4 b, out Vector4 result) {
            result.X = (a.X < b.X) ? a.X : b.X;
            result.Y = (a.Y < b.Y) ? a.Y : b.Y;
            result.Z = (a.Z < b.Z) ? a.Z : b.Z;
            result.W = (a.W < b.W) ? a.W : b.W;
        }

        /// <summary>
        /// Restricts the source vector between the range of the minimum and maximum vectors.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="min">Minimum vector</param>
        /// <param name="max">Maximum vector</param>
        /// <returns>Resulting vector in range</returns>
        public static Vector4 Clamp(Vector4 value, Vector4 min, Vector4 max) {
            Vector4 v;
            float x = value.X;
            float y = value.Y;
            float z = value.Z;
            float w = value.W;

            x = (x > max.X) ? max.X : x;
            x = (x < min.X) ? min.X : x;

            y = (y > max.Y) ? max.Y : y;
            y = (y < min.Y) ? min.Y : y;

            z = (z > max.Z) ? max.Z : z;
            z = (z < min.Z) ? min.Z : z;

            w = (w > max.W) ? max.W : w;
            w = (w < min.W) ? min.W : w;

            v.X = x;
            v.Y = y;
            v.Z = z;
            v.W = w;
            return v;
        }

        /// <summary>
        /// Restricts the source vector between the range of the minimum and maximum vectors.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="min">Minimum vector</param>
        /// <param name="max">Maximum vector</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Clamp(ref Vector4 value, ref Vector4 min, ref Vector4 max, out Vector4 result) {
            float x = value.X;
            float y = value.Y;
            float z = value.Z;
            float w = value.W;

            x = (x > max.X) ? max.X : x;
            x = (x < min.X) ? min.X : x;

            y = (y > max.Y) ? max.Y : y;
            y = (y < min.Y) ? min.Y : y;

            z = (z > max.Z) ? max.Z : z;
            z = (z < min.Z) ? min.Z : z;

            w = (w > max.W) ? max.W : w;
            w = (w < min.W) ? min.W : w;

            result.X = x;
            result.Y = y;
            result.Z = z;
            result.W = w;
        }

        /// <summary>
        /// Returns a Vector4 containing the 4D Cartesian coordinates of a point specified
        ///  in barycentric coordinates relative to a 4D triangle.
        /// </summary>
        /// <param name="a">Vector4 containing 4D cartesian coordinates corresponding to triangle's first vertex</param>
        /// <param name="b">Vector4 containing 4D cartesian coordinates corresponding to triangle's second vertex</param>
        /// <param name="c">Vector4 containing 4D cartesian coordinates corresponding to triangle's third vertex</param>
        /// <param name="b2">Barycentric coordinate b2 that is the weighting factor for the second vertex</param>
        /// <param name="b3">Barycentric coordinate b3 that is the weighting factor for the third vertex</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 Barycentric(Vector4 a, Vector4 b, Vector4 c, float b2, float b3) {
            Vector4 v;
            v.X = (a.X + (b2 * (b.X - a.X))) + (b3 * (c.X - a.X));
            v.Y = (a.Y + (b2 * (b.Y - a.Y))) + (b3 * (c.Y - a.Y));
            v.Z = (a.Z + (b2 * (b.Z - a.Z))) + (b3 * (c.Z - a.Z));
            v.W = (a.W + (b2 * (b.W - a.W))) + (b3 * (c.W - a.W));
            return v;
        }

        /// <summary>
        /// Returns a Vector4 containing the 3D Cartesian coordinates of a point specified
        ///  in barycentric coordinates relative to a 3D triangle.
        /// </summary>
        /// <param name="a">Vector4 containing 3D cartesian coordinates corresponding to triangle's first vertex</param>
        /// <param name="b">Vector4 containing 3D cartesian coordinates corresponding to triangle's second vertex</param>
        /// <param name="c">Vector4 containing 3D cartesian coordinates corresponding to triangle's third vertex</param>
        /// <param name="b2">Barycentric coordinate b2 that is the weighting factor for the second vertex</param>
        /// <param name="b3">Barycentric coordinate b3 that is the weighting factor for the third vertex</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Barycentric(ref Vector4 a, ref Vector4 b, ref Vector4 c, float b2, float b3, out Vector4 result) {
            result.X = (a.X + (b2 * (b.X - a.X))) + (b3 * (c.X - a.X));
            result.Y = (a.Y + (b2 * (b.Y - a.Y))) + (b3 * (c.Y - a.Y));
            result.Z = (a.Z + (b2 * (b.Z - a.Z))) + (b3 * (c.Z - a.Z));
            result.W = (a.W + (b2 * (b.W - a.W))) + (b3 * (c.W - a.W));
        }

        /// <summary>
        /// Compute Catmull-Rom interpolation using the the specified positions.
        /// </summary>
        /// <param name="a">First position</param>
        /// <param name="b">Second position</param>
        /// <param name="c">Third position</param>
        /// <param name="d">Fourth position</param>
        /// <param name="wf">Weighting factor</param>
        /// <returns>Resulting interpolated vector</returns>
        public static Vector4 CatmullRom(Vector4 a, Vector4 b, Vector4 c, Vector4 d, float wf) {
            Vector4 v;
            float wfSquared = wf * wf;
            float wfCubed = wf * wfSquared;
            v.X = 0.5f * ((((2.0f * b.X) + ((-a.X + c.X) * wf)) + (((((2.0f * a.X) - (5.0f * b.X)) + (4.0f * c.X)) - d.X) * wfSquared)) + ((((-a.X + (3.0f * b.X)) - (3.0f * c.X)) + d.X) * wfCubed));
            v.Y = 0.5f * ((((2.0f * b.Y) + ((-a.Y + c.Y) * wf)) + (((((2.0f * a.Y) - (5.0f * b.Y)) + (4.0f * c.Y)) - d.Y) * wfSquared)) + ((((-a.Y + (3.0f * b.Y)) - (3.0f * c.Y)) + d.Y) * wfCubed));
            v.Z = 0.5f * ((((2.0f * b.Z) + ((-a.Z + c.Z) * wf)) + (((((2.0f * a.Z) - (5.0f * b.Z)) + (4.0f * c.Z)) - d.Z) * wfSquared)) + ((((-a.Z + (3.0f * b.Z)) - (3.0f * c.Z)) + d.Z) * wfCubed));
            v.W = 0.5f * ((((2.0f * b.W) + ((-a.W + c.W) * wf)) + (((((2.0f * a.W) - (5.0f * b.W)) + (4.0f * c.W)) - d.W) * wfSquared)) + ((((-a.W + (3.0f * b.W)) - (3.0f * c.W)) + d.W) * wfCubed));
            return v;
        }

        /// <summary>
        /// Compute Catmull-Rom interpolation.
        /// </summary>
        /// <param name="a">First position</param>
        /// <param name="b">Second position</param>
        /// <param name="c">Third position</param>
        /// <param name="d">Fourth position</param>
        /// <param name="wf">Weighting factor</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void CatmullRom(ref Vector4 a, ref Vector4 b, ref Vector4 c, ref Vector4 d, float wf, out Vector4 result) {
            float wfSquared = wf * wf;
            float wfCubed = wf * wfSquared;
            result.X = 0.5f * ((((2.0f * b.X) + ((-a.X + c.X) * wf)) + (((((2.0f * a.X) - (5.0f * b.X)) + (4.0f * c.X)) - d.X) * wfSquared)) + ((((-a.X + (3.0f * b.X)) - (3.0f * c.X)) + d.X) * wfCubed));
            result.Y = 0.5f * ((((2.0f * b.Y) + ((-a.Y + c.Y) * wf)) + (((((2.0f * a.Y) - (5.0f * b.Y)) + (4.0f * c.Y)) - d.Y) * wfSquared)) + ((((-a.Y + (3.0f * b.Y)) - (3.0f * c.Y)) + d.Y) * wfCubed));
            result.Z = 0.5f * ((((2.0f * b.Z) + ((-a.Z + c.Z) * wf)) + (((((2.0f * a.Z) - (5.0f * b.Z)) + (4.0f * c.Z)) - d.Z) * wfSquared)) + ((((-a.Z + (3.0f * b.Z)) - (3.0f * c.Z)) + d.Z) * wfCubed));
            result.W = 0.5f * ((((2.0f * b.W) + ((-a.W + c.W) * wf)) + (((((2.0f * a.W) - (5.0f * b.W)) + (4.0f * c.W)) - d.W) * wfSquared)) + ((((-a.W + (3.0f * b.W)) - (3.0f * c.W)) + d.W) * wfCubed));
        }

        /// <summary>
        /// Compute a Hermite spline interpolation.
        /// </summary>
        /// <param name="a">First position</param>
        /// <param name="tangentA">First vector's tangent</param>
        /// <param name="b">Second position</param>
        /// <param name="tangentB">Second vector's tangent</param>
        /// <param name="wf">Weighting factor</param>
        /// <returns>Resulting interpolated vector</returns>
        public static Vector4 Hermite(Vector4 a, Vector4 tangentA, Vector4 b, Vector4 tangentB, float wf) {
            float wfSquared = wf * wf;
            float wfCubed = wfSquared * wf;
            float h1 = ((2.0f * wfCubed) - (3.0f * wfSquared)) + 1.0f;
            float h2 = (-2.0f * wfCubed) + (3.0f * wfSquared);
            float h3 = (wfCubed - (2.0f * wfSquared)) + wf;
            float h4 = wfCubed - wfSquared;

            Vector4 v;
            v.X = (((a.X * h1) + (b.X * h2)) + (tangentA.X * h3)) + (tangentB.X * h4);
            v.Y = (((a.Y * h1) + (b.Y * h2)) + (tangentA.Y * h3)) + (tangentB.Y * h4);
            v.Z = (((a.Z * h1) + (b.Z * h2)) + (tangentA.Z * h3)) + (tangentB.Z * h4);
            v.W = (((a.W * h1) + (b.W * h2)) + (tangentA.W * h3)) + (tangentB.W * h4);
            return v;
        }

        /// <summary>
        /// Compute a Hermite spline interpolation.
        /// </summary>
        /// <param name="a">First position</param>
        /// <param name="tangentA">First vector's tangent</param>
        /// <param name="b">Second position</param>
        /// <param name="tangentB">Second vector's tangent</param>
        /// <param name="wf">Weighting factor</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Hermite(ref Vector4 a, ref Vector4 tangentA, ref Vector4 b, ref Vector4 tangentB, float wf, out Vector4 result) {
            float wfSquared = wf * wf;
            float wfCubed = wfSquared * wf;
            float h1 = ((2.0f * wfCubed) - (3.0f * wfSquared)) + 1.0f;
            float h2 = (-2.0f * wfCubed) + (3.0f * wfSquared);
            float h3 = (wfCubed - (2.0f * wfSquared)) + wf;
            float h4 = wfCubed - wfSquared;

            result.X = (((a.X * h1) + (b.X * h2)) + (tangentA.X * h3)) + (tangentB.X * h4);
            result.Y = (((a.Y * h1) + (b.Y * h2)) + (tangentA.Y * h3)) + (tangentB.Y * h4);
            result.Z = (((a.Z * h1) + (b.Z * h2)) + (tangentA.Z * h3)) + (tangentB.Z * h4);
            result.W = (((a.W * h1) + (b.W * h2)) + (tangentA.W * h3)) + (tangentB.W * h4);
        }

        /// <summary>
        /// Compute a cubic interpolation between two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="wf">Weighting factor</param>
        /// <returns>Resulting interpolated vector</returns>
        public static Vector4 SmoothStep(Vector4 a, Vector4 b, float wf) {
            Vector4 v;
            float amt = (wf > 1.0f) ? 1.0f : ((wf < 0.0f) ? 0.0f : wf);
            amt = (amt * amt) * (3.0f - (2.0f * amt));
            v.X = a.X + ((b.X - a.X) * amt);
            v.Y = a.Y + ((b.Y - a.Y) * amt);
            v.Z = a.Z + ((b.Z - a.Z) * amt);
            v.W = a.W + ((b.W - a.W) * amt);
            return v;
        }

        /// <summary>
        /// Compute a cubic interpolation between two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <param name="wf">Weighting factor</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void SmoothStep(ref Vector4 a, ref Vector4 b, float wf, out Vector4 result) {
            float amt = (wf > 1.0f) ? 1.0f : ((wf < 0.0f) ? 0.0f : wf);
            amt = (amt * amt) * (3.0f - (2.0f * amt));
            result.X = a.X + ((b.X - a.X) * amt);
            result.Y = a.Y + ((b.Y - a.Y) * amt);
            result.Z = a.Z + ((b.Z - a.Z) * amt);
            result.W = a.W + ((b.W - a.W) * amt);
        }

        /// <summary>
        /// Linearly interpolates between two vectors.
        /// </summary>
        /// <param name="a">Starting vector</param>
        /// <param name="b">Ending vector</param>
        /// <param name="percent">Amount to interpolate by</param>
        /// <returns>Interpolated vector</returns>
        public static Vector4 Lerp(Vector4 a, Vector4 b, float percent) {
            Vector4 v;
            v.X = a.X + ((b.X - a.X) * percent);
            v.Y = a.Y + ((b.Y - a.Y) * percent);
            v.Z = a.Z + ((b.Z - a.Z) * percent);
            v.W = a.W + ((b.W - a.W) * percent);
            return v;
        }

        /// <summary>
        /// Linearly interpolates between two vectors.
        /// </summary>
        /// <param name="a">Starting vector</param>
        /// <param name="b">Ending vector</param>
        /// <param name="percent">Amount to interpolate by</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Lerp(ref Vector4 a, ref Vector4 b, float percent, out Vector4 result) {
            result.X = a.X + ((b.X - a.X) * percent);
            result.Y = a.Y + ((b.Y - a.Y) * percent);
            result.Z = a.Z + ((b.Z - a.Z) * percent);
            result.W = a.W + ((b.W - a.W) * percent);
        }

        /// <summary>
        /// Transfroms the specified vector by the matrix.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="m">Source matrix</param>
        /// <returns>Transformed vector</returns>
        public static Vector4 Transform(Vector4 value, Matrix m) {
            float x = (value.X * m.M11) + (value.Y * m.M21) + (value.Z * m.M31) + (value.W * m.M41);
            float y = (value.X * m.M12) + (value.Y * m.M22) + (value.Z * m.M32) + (value.W * m.M42);
            float z = (value.X * m.M13) + (value.Y * m.M23) + (value.Z * m.M33) + (value.W * m.M43);
            float w = (value.X * m.M14) + (value.Y * m.M24) + (value.Z * m.M34) + (value.W * m.M44);

            Vector4 v;
            v.X = x;
            v.Y = y;
            v.Z = z;
            v.W = w;
            return v;
        }

        /// <summary>
        /// Transfroms the specified vector by the matrix.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="m">Source matrix</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Transform(ref Vector4 value, ref Matrix m, out Vector4 result) {
            float x = (value.X * m.M11) + (value.Y * m.M21) + (value.Z * m.M31) + (value.W * m.M41);
            float y = (value.X * m.M12) + (value.Y * m.M22) + (value.Z * m.M32) + (value.W * m.M42);
            float z = (value.X * m.M13) + (value.Y * m.M23) + (value.Z * m.M33) + (value.W * m.M43);
            float w = (value.X * m.M14) + (value.Y * m.M24) + (value.Z * m.M34) + (value.W * m.M44);

            result.X = x;
            result.Y = y;
            result.Z = z;
            result.W = w;
        }

        /// <summary>
        /// Transfroms the specified vector by the quaternion.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="q">Source quaternion</param>
        /// <returns>Transformed vector</returns>
        public static Vector4 Transform(Vector4 value, Quaternion q) {
            float x2 = q.X + q.X;
            float y2 = q.Y + q.Y;
            float z2 = q.Z + q.Z;

            float wx2 = q.W * x2;
            float wy2 = q.W * y2;
            float wz2 = q.W * z2;

            float xx2 = q.X * x2;
            float xy2 = q.X * y2;
            float xz2 = q.X * z2;

            float yy2 = q.Y * y2;
            float yz2 = q.Y * z2;

            float zz2 = q.Z * z2;

            float x = ((value.X * ((1.0f - yy2) - zz2)) + (value.Y * (xy2 - wz2))) + (value.Z * (xz2 + wy2));
            float y = ((value.X * (xy2 + wz2)) + (value.Y * ((1.0f - xx2) - zz2))) + (value.Z * (yz2 - wx2));
            float z = ((value.X * (xz2 - wy2)) + (value.Y * (yz2 + wx2))) + (value.Z * ((1.0f - xx2) - yy2));

            Vector4 v;
            v.X = x;
            v.Y = y;
            v.Z = z;
            v.W = value.W;
            return v;
        }

        /// <summary>
        /// Transfroms the specified vector by the quaternion.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="q">Source quaternion</param>
        /// <param name="result">Existing vector to hold result</param>
        public static void Transform(ref Vector4 value, ref Quaternion q, out Vector4 result) {
            float x2 = q.X + q.X;
            float y2 = q.Y + q.Y;
            float z2 = q.Z + q.Z;

            float wx2 = q.W * x2;
            float wy2 = q.W * y2;
            float wz2 = q.W * z2;

            float xx2 = q.X * x2;
            float xy2 = q.X * y2;
            float xz2 = q.X * z2;

            float yy2 = q.Y * y2;
            float yz2 = q.Y * z2;

            float zz2 = q.Z * z2;

            float x = ((value.X * ((1.0f - yy2) - zz2)) + (value.Y * (xy2 - wz2))) + (value.Z * (xz2 + wy2));
            float y = ((value.X * (xy2 + wz2)) + (value.Y * ((1.0f - xx2) - zz2))) + (value.Z * (yz2 - wx2));
            float z = ((value.X * (xz2 - wy2)) + (value.Y * (yz2 + wx2))) + (value.Z * ((1.0f - xx2) - yy2));

            result.X = x;
            result.Y = y;
            result.Z = z;
            result.W = value.W;
        }  

        /// <summary>
        /// Tests equality between this vector and another vector.
        /// </summary>
        /// <param name="value">Vector to test against</param>
        /// <returns>True if components are equal</returns>
        public bool Equals(Vector4 value) {
            return ((((this.X == value.X) && (this.Y == value.Y)) && (this.Z == value.Z)) && (this.W == value.W));
        }

        /// <summary>
        /// Tests equality between two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>True if components are equal</returns>
        public static bool operator==(Vector4 a, Vector4 b) {
            return ((((a.X == b.X) && (a.Y == b.Y)) && (a.Z == b.Z)) && (a.W == b.W));
        }

        /// <summary>
        /// Tests inequality between two vectors.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>True if components are not equal</returns>
        public static bool operator!=(Vector4 a, Vector4 b) {
            return !(a == b);
        }

        /// <summary>
        /// Adds the two vectors together.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Added vector</returns>
        public static Vector4 operator+(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X + b.X;
            v.Y = a.Y + b.Y;
            v.Z = a.Z + b.Z;
            v.W = a.W + b.W;
            return v;
        }

        /// <summary>
        /// Flips the signs of the components of the specified vector.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <returns>Negated vector</returns>
        public static Vector4 operator-(Vector4 value) {
            Vector4 v;
            v.X = -value.X;
            v.Y = -value.Y;
            v.Z = -value.Z;
            v.W = -value.W;
            return v;
        }

        /// <summary>
        /// Subtracts a vector from another.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Resulting vector</returns>
        public static Vector4 operator-(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X - b.X;
            v.Y = a.Y - b.Y;
            v.Z = a.Z - b.Z;
            v.W = a.W - b.W;
            return v;
        }

        /// <summary>
        /// Multiplies two vectors together.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Multiplied vector</returns>
        public static Vector4 operator*(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X * b.X;
            v.Y = a.Y * b.Y;
            v.Z = a.Z * b.Z;
            v.W = a.W * b.W;
            return v;
        }

        /// <summary>
        /// Multiplies a vector by a scaling factor.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="scale">Amount to multiply</param>
        /// <returns>Multiplied vector</returns>
        public static Vector4 operator*(Vector4 value, float scale) {
            Vector4 v;
            v.X = value.X * scale;
            v.Y = value.Y * scale;
            v.Z = value.Z * scale;
            v.W = value.W * scale;
            return v;
        }

        /// <summary>
        /// Multiplies a vector by a scaling factor.
        /// </summary>
        /// <param name="scale">Amount to multiply</param>
        /// <param name="value">Source vector</param>
        /// <returns>Multiplied vector</returns>
        public static Vector4 operator*(float scale, Vector4 value) {
            Vector4 v;
            v.X = value.X * scale;
            v.Y = value.Y * scale;
            v.Z = value.Z * scale;
            v.W = value.W * scale;
            return v;
        }

        /// <summary>
        /// Divides the first vector by the second.
        /// </summary>
        /// <param name="a">First vector</param>
        /// <param name="b">Second vector</param>
        /// <returns>Divided vector</returns>
        public static Vector4 operator/(Vector4 a, Vector4 b) {
            Vector4 v;
            v.X = a.X / b.X;
            v.Y = a.Y / b.Y;
            v.Z = a.Z / b.Z;
            v.W = a.W / b.W;
            return v;
        }

        /// <summary>
        /// Divides a vector by a divisor value.
        /// </summary>
        /// <param name="value">Source vector</param>
        /// <param name="divisor">Divisor</param>
        /// <returns>Divided vector</returns>
        public static Vector4 operator/(Vector4 value, float divisor) {
            Vector4 v;
            float invDivisor = 1.0f / divisor;
            v.X = value.X * invDivisor;
            v.Y = value.Y * invDivisor;
            v.Z = value.Z * invDivisor;
            v.W = value.W * invDivisor;
            return v;
        }

        /// <summary>
        /// Tests equality between this vector and the supplied object.
        /// </summary>
        /// <param name="obj">Object to compare</param>
        /// <returns>True if object is a vector and components are equal</returns>
        public override bool Equals(object obj) {
            if(obj is Vector4) {
                return Equals((Vector4) obj);
            }
            return false;
        }

        /// <summary>
        /// Get the object's hash code.
        /// </summary>
        /// <returns>Hash code</returns>
        public override int GetHashCode() {
            return (this.X.GetHashCode() + this.Y.GetHashCode() + this.Z.GetHashCode() + this.W.GetHashCode());
        }

        /// <summary>
        /// Get a string representation for the object.
        /// </summary>
        /// <returns>String representation</returns>
        public override String ToString() {
            CultureInfo info = CultureInfo.CurrentCulture;
            return String.Format(info, "{{X:{0} Y:{1} Z:{2} W:{3}}}",
                new Object[] { this.X.ToString(info), this.Y.ToString(info), this.Z.ToString(info), this.W.ToString(info) });
        }
    }
}
